﻿using System;
using System.Collections.Generic;
using System.Linq;
using System.Text;
using Drifting.Foundation;

namespace Drifting.MotionPlanning.Systems
{

    public class Drift12D : BaseSystem
    {
        public class Actuator
        {
            double[] position;
            double[] unitForce;
            double[] unitTorque;

            public Actuator(double[] position, double[] unitForce)
            {
                this.position = position;

                double x = unitForce[0];
                double y = unitForce[1];
                double z = unitForce[2];
                double length = Math.Sqrt(x * x + y * y + z * z);

                unitForce[0] = x / length;
                unitForce[1] = y / length;
                unitForce[2] = z / length;
                this.unitForce = unitForce;

                unitTorque = new double[3];
                MathAssistant.vector_cross3(position, unitForce, ref unitTorque);
            }

            public Actuator(double xp, double yp, double zp, double xf, double yf, double zf)
            {
                this.position = new double[3];
                position[0] = xp;
                position[1] = yp;
                position[2] = zp;

                this.unitForce = new double[3];
                double length = Math.Sqrt(xf * xf + yf * yf + zf * zf);
                unitForce[0] = xf / length;
                unitForce[1] = yf / length;
                unitForce[2] = zf / length;
                
                unitTorque = new double[3];
                MathAssistant.vector_cross3(position, unitForce, ref unitTorque);
            }

            public void GetForce(double u, ref double f_x, ref double f_y, ref double f_z)
            {
                f_x = unitForce[0] * u;
                f_y = unitForce[1] * u;
                f_z = unitForce[2] * u;
            }

            public void GetTorque(double u, ref double t_x, ref double t_y, ref double t_z)
            {
                t_x = unitTorque[0] * u;
                t_y = unitTorque[1] * u;
                t_z = unitTorque[2] * u;
            }
        }

        List<Actuator> engines;
        double M;
        double Jx;
        double Jy;
        double Jz;

        public Drift12D()
            : base("../../../MotionPlanning/Resources/Drift12D.kdl")
        {
            engines = new List<Actuator>();
            engines.Add(new Actuator(-1, 0, 0, 1, 0, 0));

            engines.Add(new Actuator(-1, 0, 0.5,      0, 1, 0));
            engines.Add(new Actuator(-1, 0, -0.5,      0, 1, 0));

            engines.Add(new Actuator(-1, 0.5, 0,     0, 0, 1));
            engines.Add(new Actuator(-1, -0.5, 0,    0, 0, 1));

            M = 1;
            Jx = 1;
            Jy = 1;
            Jz = 1;

            t_delta = 0.01;
        }

        override public void Qd(double[] q, double[] ctrl, ref double[] qd)
        {
            throw new NotImplementedException();
        }

        public void Qd(    double q0, double q1, double q2, double w, 
                           double[] ctrl, 
                           ref double A_x_world, ref double A_y_world, ref double A_z_world,
                           ref double B_x_world, ref double B_y_world, ref double B_z_world)
        {
            double FSum_x = 0;
            double FSum_y = 0;
            double FSum_z = 0;
            double TSum_x = 0;
            double TSum_y = 0;
            double TSum_z = 0;

            double u_i = 0;
            double x_i = 0;
            double y_i = 0;
            double z_i = 0;

            for (int i = 0; i < engines.Count; ++i)
            {
                u_i = ctrl[i];

                Actuator engine = engines[i];
                engine.GetForce(u_i, ref x_i, ref y_i, ref z_i);
                FSum_x += x_i;
                FSum_y += y_i;
                FSum_z += z_i;
                engine.GetTorque(u_i, ref x_i, ref y_i, ref z_i);
                TSum_x += x_i;
                TSum_y += y_i;
                TSum_z += z_i;
            }

            // fight local frame 
            double A_x = FSum_x / M;
            double A_y = FSum_y / M;
            double A_z = FSum_z / M;
            double B_x = TSum_x / Jx;
            double B_y = TSum_y / Jy;
            double B_z = TSum_z / Jz;

            // matrix box to world
            double M00 = 0; double M01 = 0; double M02 = 0;
            double M10 = 0; double M11 = 0; double M12 = 0;
            double M20 = 0; double M21 = 0; double M22 = 0;

            MathAssistant.CreateMatrixFromQuaternion(q0, q1, q2, w, ref M00, ref M01, ref M02, ref M10, ref M11, ref M12, ref M20, ref M21, ref M22);
            
            A_x_world = M00 * A_x + M01 * A_y + M02 * A_z;
            A_y_world = M10 * A_x + M11 * A_y + M12 * A_z;
            A_z_world = M20 * A_x + M21 * A_y + M22 * A_z;

            B_x_world = M00 * B_x + M01 * B_y + M02 * B_z;
            B_y_world = M10 * B_x + M11 * B_y + M12 * B_z;
            B_z_world = M20 * B_x + M21 * B_y + M22 * B_z;
        }

        override public void Integration(MemoryPad pad, double[] q, double[] ctrl, double t_delta, ref double[] q_next)
        {
            //current orientation
            double q0 = q[3];
            double q1 = q[4];
            double q2 = q[5];
            double qw = q[6];

            //accelerations
            double A_x_world = 0;
            double A_y_world = 0;
            double A_z_world = 0;
            double B_x_world = 0;
            double B_y_world = 0;
            double B_z_world = 0;
            Qd(q0, q1, q2, qw, ctrl, ref A_x_world, ref A_y_world, ref A_z_world, ref B_x_world, ref B_y_world, ref B_z_world);

            //new position
            q_next[0] += q[7] * t_delta;
            q_next[1] += q[8] * t_delta;
            q_next[2] += q[9] * t_delta;

            //delta orientation
            double dx0 = q[10] * t_delta;
            double dx1 = q[11] * t_delta;
            double dx2 = q[12] * t_delta;
            double length_w = Math.Sqrt(dx0 * dx0 + dx1 * dx1 + dx2 * dx2);

            //new orientation
            double cw = 0;
            double c0 = 0;
            double c1 = 0;
            double c2 = 0;

            if (length_w > 0.0000000001)
            {
                //delta orientation in quaternion form
                double dq0 = 0;
                double dq1 = 0;
                double dq2 = 0;
                double dw = 0;
                MathAssistant.QuaternionFromAxisAngle(dx0, dx1, dx2, length_w, ref dq0, ref dq1, ref dq2, ref dw);
                MathAssistant.QuaternionProduct(dw, dq0, dq1, dq2,
                                                qw, q0, q1, q2,
                                                ref cw, ref c0, ref c1, ref c2);
            }
            else
            {
                c0 = q0;
                c1 = q1;
                c2 = q2;
                cw = qw;
            }

            MathAssistant.QuaternionNormalize(ref c0, ref c1, ref c2, ref cw);

            q_next[3] = c0;
            q_next[4] = c1;
            q_next[5] = c2;
            q_next[6] = cw;

            //update velocities
            q_next[7] += A_x_world * t_delta;
            q_next[8] += A_y_world * t_delta;
            q_next[9] += A_z_world * t_delta;
            q_next[10] += B_x_world * t_delta;
            q_next[11] += B_y_world * t_delta;
            q_next[12] += B_z_world * t_delta;
        }

        public override void SampleAlias(Random rand, ref double[] alias)
        {
            base.SampleAlias(rand, ref alias);

            double q0 = alias[3];
            double q1 = alias[4];
            double q2 = alias[5];
            double qw = alias[6];
            double length = Math.Sqrt(q0 * q0 + q1 * q1 + q2 * q2 + qw * qw);
            if (length < 0.0000001) throw new ArgumentException();
            alias[3] /= length;
            alias[4] /= length;
            alias[5] /= length;
            alias[6] /= length;
        }
    }
}
